#include "constants.h"
#include "MyPose.h"

#ifndef COSTMARKER_H_
#define COSTMARKER_H_

typedef PoseArray Configuration;
typedef std::vector<Configuration> RobotSet;

class CostMarker {
	visualization_msgs::Marker point;
public:
	CostMarker();
	void Plot(MATRIX &,ros::Publisher &,double grain = 0.1);
	void PlotParticles(RobotSet &S, ros::Publisher &pub);
	void PlotPoses(int x, int y, ros::Publisher &pub, double grain);
	void PlotPath(std::vector<PII> path, ros::Publisher &pub, double grain);
	void PlotPoseProbability(PoseArray &pa, WeightsForPoses &w, ros::Publisher &pub, double grain);
	virtual ~CostMarker();
};

#endif /* COSTMARKER_H_ */
